﻿using System;
using System.Diagnostics;
using System.Globalization;
using TomanuExtensions;
using YAXLib;

namespace RaytracerLib.MathLib
{
    /// <summary>
    /// For 3D.
    /// </summary>
    [DebuggerStepThrough]
    public struct Matrix4
    {
        [YAXAttributeForClass]
        public readonly double M11, M12, M13, M14;

        [YAXAttributeForClass]
        public readonly double M21, M22, M23, M24;

        [YAXAttributeForClass]
        public readonly double M31, M32, M33, M34;

        [YAXAttributeForClass]
        public readonly double M41, M42, M43, M44;

        public static readonly Matrix4 Identity = new Matrix4(

            1, 0, 0, 0,
            0, 1, 0, 0,
            0, 0, 1, 0,
            0, 0, 0, 1);

        public Matrix4(double a_m11, double a_m12, double a_m13, double a_m14,
                       double a_m21, double a_m22, double a_m23, double a_m24,
                       double a_m31, double a_m32, double a_m33, double a_m34,
                       double a_m41, double a_m42, double a_m43, double a_m44)
        {
            M11 = a_m11;
            M12 = a_m12;
            M13 = a_m13;
            M14 = a_m14;

            M21 = a_m21;
            M22 = a_m22;
            M23 = a_m23;
            M24 = a_m24;

            M31 = a_m31;
            M32 = a_m32;
            M33 = a_m33;
            M34 = a_m34;

            M41 = a_m41;
            M42 = a_m42;
            M43 = a_m43;
            M44 = a_m44;
        }

        public Matrix4(double[] a_ar)
        {
            Debug.Assert(a_ar.Length == 16);

            M11 = a_ar[0];
            M12 = a_ar[1];
            M13 = a_ar[2];
            M14 = a_ar[3];

            M21 = a_ar[4];
            M22 = a_ar[5];
            M23 = a_ar[6];
            M24 = a_ar[7];

            M31 = a_ar[8];
            M32 = a_ar[9];
            M33 = a_ar[10];
            M34 = a_ar[11];

            M41 = a_ar[12];
            M42 = a_ar[13];
            M43 = a_ar[14];
            M44 = a_ar[15];
        }

        public Matrix4(Matrix4 a_m)
        {
            M11 = a_m.M11;
            M12 = a_m.M12;
            M13 = a_m.M13;
            M14 = a_m.M14;

            M21 = a_m.M21;
            M22 = a_m.M22;
            M23 = a_m.M23;
            M24 = a_m.M24;

            M31 = a_m.M31;
            M32 = a_m.M32;
            M33 = a_m.M33;
            M34 = a_m.M34;

            M41 = a_m.M41;
            M42 = a_m.M42;
            M43 = a_m.M43;
            M44 = a_m.M44;
        }

        public Matrix4(Vector3 a_col1, Vector3 a_col2, Vector3 a_col3)
        {
            M11 = a_col1.X;
            M21 = a_col1.Y;
            M31 = a_col1.Z;
            M41 = 0;

            M12 = a_col2.X;
            M22 = a_col2.Y;
            M32 = a_col2.Z;
            M42 = 0;

            M13 = a_col3.X;
            M23 = a_col3.Y;
            M33 = a_col3.Z;
            M43 = 0;

            M14 = 0;
            M24 = 0;
            M34 = 0;
            M44 = 1;
        }

        public Matrix4(Vector3 a_col1, Vector3 a_col2, Vector3 a_col3, Vector3 a_col4)
        {
            M11 = a_col1.X;
            M21 = a_col1.Y;
            M31 = a_col1.Z;
            M41 = 0;

            M12 = a_col2.X;
            M22 = a_col2.Y;
            M32 = a_col2.Z;
            M42 = 0;

            M13 = a_col3.X;
            M23 = a_col3.Y;
            M33 = a_col3.Z;
            M43 = 0;

            M14 = a_col4.X;
            M24 = a_col4.Y;
            M34 = a_col4.Z;
            M44 = 1;
        }

        public static Matrix4 operator +(Matrix4 a_a, Matrix4 a_b)
        {
            return new Matrix4(

                a_a.M11 + a_b.M11,
                a_a.M12 + a_b.M12,
                a_a.M13 + a_b.M13,
                a_a.M14 + a_b.M14,

                a_a.M21 + a_b.M21,
                a_a.M22 + a_b.M22,
                a_a.M23 + a_b.M23,
                a_a.M24 + a_b.M24,

                a_a.M31 + a_b.M31,
                a_a.M32 + a_b.M32,
                a_a.M33 + a_b.M33,
                a_a.M34 + a_b.M34,

                a_a.M41 + a_b.M41,
                a_a.M42 + a_b.M42,
                a_a.M43 + a_b.M43,
                a_a.M44 + a_b.M44);
        }

        public Matrix4 Transposed
        {
            get
            {
                return new Matrix4(

                    M11, M21, M31, M41,
                    M12, M22, M32, M42,
                    M13, M23, M33, M43,
                    M14, M24, M34, M44);
            }
        }

        public Matrix4 Inverted
        {
            get
            {
                var d01 = M31 * M42 - M41 * M32;
                var d02 = M31 * M43 - M41 * M33;
                var d12 = M32 * M43 - M42 * M33;
                var d13 = M32 * M44 - M42 * M34;
                var d23 = M33 * M44 - M43 * M34;
                var d30 = M34 * M41 - M44 * M31;

                double r_11 = M22 * d23 - M23 * d13 + M24 * d12;
                double r_21 = -(M21 * d23 + M23 * d30 + M24 * d02);
                double r_31 = M21 * d13 + M22 * d30 + M24 * d01;
                double r_41 = -(M21 * d12 - M22 * d02 + M23 * d01);

                var s = 1 / (M11 * r_11 + M12 * r_21 + M13 * r_31 + M14 * r_41);

                double r_12 = -(M12 * d23 - M13 * d13 + M14 * d12) * s;
                double r_22 = (M11 * d23 + M13 * d30 + M14 * d02) * s;
                double r_32 = -(M11 * d13 + M12 * d30 + M14 * d01) * s;
                double r_42 = (M11 * d12 - M12 * d02 + M13 * d01) * s;

                d01 = M11 * M22 - M21 * M12;
                d02 = M11 * M23 - M21 * M13;
                d12 = M12 * M23 - M22 * M13;
                d13 = M12 * M24 - M22 * M14;
                d23 = M13 * M24 - M23 * M14;
                d30 = M14 * M21 - M24 * M11;

                return new Matrix4(
                    r_11 * s,
                    r_12,
                    (M42 * d23 - M43 * d13 + M44 * d12) * s,
                    -(M32 * d23 - M33 * d13 + M34 * d12) * s,

                    r_21 * s,
                    r_22,
                    -(M41 * d23 + M43 * d30 + M44 * d02) * s,
                    (M31 * d23 + M33 * d30 + M34 * d02) * s,

                    r_31 * s,
                    r_32,
                    (M41 * d13 + M42 * d30 + M44 * d01) * s,
                    -(M31 * d13 + M32 * d30 + M34 * d01) * s,

                    r_41 * s,
                    r_42,
                    -(M41 * d12 - M42 * d02 + M43 * d01) * s,
                    (M31 * d12 - M32 * d02 + M33 * d01) * s
                );
            }
        }

        public Matrix4 Rotation
        {
            get
            {
                return new Matrix4(M11, M12, M13, 0,
                                   M21, M22, M23, 0,
                                   M31, M32, M33, 0,
                                   0, 0, 0, 1);
            }
        }

        public Matrix4 Translation
        {
            get
            {
                return Rotation.Inverted * this;
            }
        }

        public double Determinant
        {
            get
            {
                var det1 = (M33 * M44) - (M34 * M43);
                var det2 = (M32 * M44) - (M34 * M42);
                var det3 = (M32 * M43) - (M33 * M42);
                var det4 = (M31 * M44) - (M34 * M41);
                var det5 = (M31 * M43) - (M33 * M41);
                var det6 = (M31 * M42) - (M32 * M41);

                return M11 * (M22 * det1 - M23 * det2 + M24 * det3) -
                       M12 * (M21 * det1 - M23 * det4 + M24 * det5) +
                       M13 * (M21 * det2 - M22 * det4 + M24 * det6) -
                       M14 * (M21 * det3 - M22 * det5 + M23 * det6);
            }
        }

        public static Matrix4 operator -(Matrix4 a_a, Matrix4 a_b)
        {
            return new Matrix4(

                a_a.M11 - a_b.M11,
                a_a.M12 - a_b.M12,
                a_a.M13 - a_b.M13,
                a_a.M14 - a_b.M14,

                a_a.M21 - a_b.M21,
                a_a.M22 - a_b.M22,
                a_a.M23 - a_b.M23,
                a_a.M24 - a_b.M24,

                a_a.M31 - a_b.M31,
                a_a.M32 - a_b.M32,
                a_a.M33 - a_b.M33,
                a_a.M34 - a_b.M34,

                a_a.M41 - a_b.M41,
                a_a.M42 - a_b.M42,
                a_a.M43 - a_b.M43,
                a_a.M44 - a_b.M44);
        }

        public static Matrix4 operator *(Matrix4 a_a, Matrix4 a_b)
        {
            return new Matrix4(

                a_a.M11 * a_b.M11 + a_a.M12 * a_b.M21 +
                a_a.M13 * a_b.M31 + a_a.M14 * a_b.M41,
                a_a.M11 * a_b.M12 + a_a.M12 * a_b.M22 +
                a_a.M13 * a_b.M32 + a_a.M14 * a_b.M42,
                a_a.M11 * a_b.M13 + a_a.M12 * a_b.M23 +
                a_a.M13 * a_b.M33 + a_a.M14 * a_b.M43,
                a_a.M11 * a_b.M14 + a_a.M12 * a_b.M24 +
                a_a.M13 * a_b.M34 + a_a.M14 * a_b.M44,

                a_a.M21 * a_b.M11 + a_a.M22 * a_b.M21 +
                a_a.M23 * a_b.M31 + a_a.M24 * a_b.M41,
                a_a.M21 * a_b.M12 + a_a.M22 * a_b.M22 +
                a_a.M23 * a_b.M32 + a_a.M24 * a_b.M42,
                a_a.M21 * a_b.M13 + a_a.M22 * a_b.M23 +
                a_a.M23 * a_b.M33 + a_a.M24 * a_b.M43,
                a_a.M21 * a_b.M14 + a_a.M22 * a_b.M24 +
                a_a.M23 * a_b.M34 + a_a.M24 * a_b.M44,

                a_a.M31 * a_b.M11 + a_a.M32 * a_b.M21 +
                a_a.M33 * a_b.M31 + a_a.M34 * a_b.M41,
                a_a.M31 * a_b.M12 + a_a.M32 * a_b.M22 +
                a_a.M33 * a_b.M32 + a_a.M34 * a_b.M42,
                a_a.M31 * a_b.M13 + a_a.M32 * a_b.M23 +
                a_a.M33 * a_b.M33 + a_a.M34 * a_b.M43,
                a_a.M31 * a_b.M14 + a_a.M32 * a_b.M24 +
                a_a.M33 * a_b.M34 + a_a.M34 * a_b.M44,

                a_a.M41 * a_b.M11 + a_a.M42 * a_b.M21 +
                a_a.M43 * a_b.M31 + a_a.M44 * a_b.M41,
                a_a.M41 * a_b.M12 + a_a.M42 * a_b.M22 +
                a_a.M43 * a_b.M32 + a_a.M44 * a_b.M42,
                a_a.M41 * a_b.M13 + a_a.M42 * a_b.M23 +
                a_a.M43 * a_b.M33 + a_a.M44 * a_b.M43,
                a_a.M41 * a_b.M14 + a_a.M42 * a_b.M24 +
                a_a.M43 * a_b.M34 + a_a.M44 * a_b.M44);
        }

        public static Vector3 operator *(Matrix4 a_a, Vector3 a_b)
        {
            double x = (a_a.M11 * a_b.X) +
                      (a_a.M12 * a_b.Y) +
                      (a_a.M13 * a_b.Z) +
                      a_a.M14;
            double y = (a_a.M21 * a_b.X) +
                      (a_a.M22 * a_b.Y) +
                      (a_a.M23 * a_b.Z) +
                      a_a.M24;
            double z = (a_a.M31 * a_b.X) +
                      (a_a.M32 * a_b.Y) +
                      (a_a.M33 * a_b.Z) +
                      a_a.M34;
            double w = (a_a.M41 * a_b.X) +
                      (a_a.M42 * a_b.Y) +
                      (a_a.M43 * a_b.Z) +
                      a_a.M44;

            return new Vector3(x / w, y / w, z / w);
        }

        public override int GetHashCode()
        {
            return M11.GetHashCode() ^ M12.GetHashCode() ^
                   M13.GetHashCode() ^ M14.GetHashCode() ^
                   M21.GetHashCode() ^ M22.GetHashCode() ^
                   M23.GetHashCode() ^ M24.GetHashCode() ^
                   M31.GetHashCode() ^ M32.GetHashCode() ^
                   M33.GetHashCode() ^ M34.GetHashCode() ^
                   M41.GetHashCode() ^ M42.GetHashCode() ^
                   M43.GetHashCode() ^ M44.GetHashCode();
        }

        public override bool Equals(object a_obj)
        {
            if (a_obj == null)
                return false;

            if (!typeof(Matrix4).Equals(a_obj.GetType()))
                return false;
            Matrix4 matrix3d4 = (Matrix4)a_obj;

            return (M11 == matrix3d4.M11) &&
                   (M12 == matrix3d4.M12) &&
                   (M13 == matrix3d4.M13) &&
                   (M14 == matrix3d4.M14) &&

                   (M21 == matrix3d4.M21) &&
                   (M22 == matrix3d4.M22) &&
                   (M23 == matrix3d4.M23) &&
                   (M24 == matrix3d4.M24) &&

                   (M31 == matrix3d4.M31) &&
                   (M32 == matrix3d4.M32) &&
                   (M33 == matrix3d4.M33) &&
                   (M34 == matrix3d4.M34) &&

                   (M41 == matrix3d4.M41) &&
                   (M42 == matrix3d4.M42) &&
                   (M43 == matrix3d4.M43) &&
                   (M44 == matrix3d4.M44);
        }

        public bool Equals(Matrix4 a_matrix3d4)
        {
            return (M11 == a_matrix3d4.M11) &&
                   (M12 == a_matrix3d4.M12) &&
                   (M13 == a_matrix3d4.M13) &&
                   (M14 == a_matrix3d4.M14) &&

                   (M21 == a_matrix3d4.M21) &&
                   (M22 == a_matrix3d4.M22) &&
                   (M23 == a_matrix3d4.M23) &&
                   (M24 == a_matrix3d4.M24) &&

                   (M31 == a_matrix3d4.M31) &&
                   (M32 == a_matrix3d4.M32) &&
                   (M33 == a_matrix3d4.M33) &&
                   (M34 == a_matrix3d4.M34) &&

                   (M41 == a_matrix3d4.M41) &&
                   (M42 == a_matrix3d4.M42) &&
                   (M43 == a_matrix3d4.M43) &&
                   (M44 == a_matrix3d4.M44);
        }

        public bool IsAlmostRelativeEquals(Matrix4 a_m, double a_precision = Constants.DOUBLE_PRECISION)
        {
            for (int i = 0; i < 16; i++)
            {
                if (!this[i].IsAlmostRelativeEquals(a_m[i], a_precision))
                    return false;
            }

            return true;
        }

        public double this[int a_index]
        {
            get
            {
                switch (a_index)
                {
                    case 0: return M11;
                    case 1: return M12;
                    case 2: return M13;
                    case 3: return M14;

                    case 4: return M21;
                    case 5: return M22;
                    case 6: return M23;
                    case 7: return M24;

                    case 8: return M31;
                    case 9: return M32;
                    case 10: return M33;
                    case 11: return M34;

                    case 12: return M41;
                    case 13: return M42;
                    case 14: return M43;
                    case 15: return M44;
                }

                throw new IndexOutOfRangeException();
            }
        }

        public double this[int a_x, int a_y]
        {
            get
            {
                Debug.Assert(a_x >= 0);
                Debug.Assert(a_y >= 0);
                Debug.Assert(a_x < 4);
                Debug.Assert(a_y < 4);

                return this[a_y * 4 + a_x];
            }
        }

        public override string ToString()
        {
            const int SIZE = 4;

            string r = "";

            for (int y = 0; y < SIZE; y++)
            {
                r = r + "( ";

                for (int x = 0; x < SIZE; x++)
                {
                    r = r + this[y * SIZE + x].ToString(CultureInfo.InvariantCulture);

                    if (x != SIZE - 1)
                        r = r + ", ";
                }

                r = r + " ) ";
            }

            return r;
        }

        public static bool operator ==(Matrix4 a_a, Matrix4 a_b)
        {
            return (a_a.M11 == a_b.M11) &&
                   (a_a.M12 == a_b.M12) &&
                   (a_a.M13 == a_b.M13) &&
                   (a_a.M14 == a_b.M14) &&

                   (a_a.M21 == a_b.M21) &&
                   (a_a.M22 == a_b.M22) &&
                   (a_a.M23 == a_b.M23) &&
                   (a_a.M24 == a_b.M24) &&

                   (a_a.M31 == a_b.M31) &&
                   (a_a.M32 == a_b.M32) &&
                   (a_a.M33 == a_b.M33) &&
                   (a_a.M34 == a_b.M34) &&

                   (a_a.M41 == a_b.M41) &&
                   (a_a.M42 == a_b.M42) &&
                   (a_a.M43 == a_b.M43) &&
                   (a_a.M44 == a_b.M44);
        }

        public static bool operator !=(Matrix4 a_a, Matrix4 a_b)
        {
            return !(a_a == a_b);
        }

        public static Matrix4 CreateTranslation(Vector3 a_v)
        {
            return CreateTranslation(a_v.X, a_v.Y, a_v.Z);
        }

        public static Matrix4 CreateTranslation(double a_x, double a_y, double a_z)
        {
            return new Matrix4(

                1, 0, 0, a_x,
                0, 1, 0, a_y,
                0, 0, 1, a_z,
                0, 0, 0, 1);
        }

        public Vector3 Col1
        {
            get
            {
                return new Vector3(M11, M21, M31);
            }
        }

        public Matrix4 SetCol1(Vector3 a_v)
        {
            return new Matrix4(

                a_v.X,
                M12,
                M13,
                M14,

                a_v.Y,
                M22,
                M23,
                M24,

                a_v.Z,
                M32,
                M33,
                M34,

                M41,
                M42,
                M43,
                M44);
        }

        public Vector3 Col2
        {
            get
            {
                return new Vector3(M12, M22, M32);
            }
        }

        public Matrix4 SetCol2(Vector3 a_v)
        {
            return new Matrix4(

                M11,
                a_v.X,
                M13,
                M14,

                M21,
                a_v.Y,
                M23,
                M24,

                M31,
                a_v.Z,
                M33,
                M34,

                M41,
                M42,
                M43,
                M44);
        }

        public Vector3 Col3
        {
            get
            {
                return new Vector3(M13, M23, M33);
            }
        }

        public Matrix4 SetCol3(Vector3 a_v)
        {
            return new Matrix4(

                M11,
                M12,
                a_v.X,
                M14,

                M21,
                M22,
                a_v.Y,
                M24,

                M31,
                M32,
                a_v.Z,
                M34,

                M41,
                M42,
                M43,
                M44);
        }

        public Vector3 Col4
        {
            get
            {
                return new Vector3(M14, M24, M34);
            }
        }

        public Matrix4 SetCol4(Vector3 a_v)
        {
            return new Matrix4(

                M11,
                M12,
                M13,
                a_v.X,

                M21,
                M22,
                M23,
                a_v.Y,

                M31,
                M32,
                M33,
                a_v.Z,

                M41,
                M42,
                M43,
                M44);
        }

        /// <summary>
        /// Angle in deg.
        /// </summary>
        /// <param name="a_angle"></param>
        /// <returns></returns>
        public static Matrix4 CreateRotateX(double a_angle)
        {
            double sin = Math.Sin(MathExtensions.ToRad(a_angle));
            double cos = Math.Cos(MathExtensions.ToRad(a_angle));

            return new Matrix4(

                1, 0, 0, 0,
                0, cos, -sin, 0,
                0, sin, cos, 0,
                0, 0, 0, 1);
        }

        /// <summary>
        /// Angle in deg.
        /// </summary>
        /// <param name="a_angle"></param>
        /// <returns></returns>
        public static Matrix4 CreateRotateY(double a_angle)
        {
            double sin = Math.Sin(MathExtensions.ToRad(a_angle));
            double cos = Math.Cos(MathExtensions.ToRad(a_angle));

            return new Matrix4(

                cos, 0, sin, 0,
                0, 1, 0, 0,
                -sin, 0, cos, 0,
                0, 0, 0, 1);
        }

        /// <summary>
        /// Angle in deg.
        /// </summary>
        /// <param name="a_angle"></param>
        /// <returns></returns>
        public static Matrix4 CreateRotateZ(double a_angle)
        {
            double sin = Math.Sin(MathExtensions.ToRad(a_angle));
            double cos = Math.Cos(MathExtensions.ToRad(a_angle));

            return new Matrix4(

                cos, -sin, 0, 0,
                sin, cos, 0, 0,
                0, 0, 1, 0,
                0, 0, 0, 1);
        }

        /// <summary>
        /// Angle in deg.
        /// </summary>
        /// <param name="a_rotate_around"></param>
        /// <param name="a_angle"></param>
        /// <returns></returns>
        public static Matrix4 CreateRotateAroundVector(Vector3 a_rotate_around,
            double a_angle)
        {
            a_rotate_around = a_rotate_around.Normalized;

            double cos = Math.Cos(MathExtensions.ToRad(a_angle));
            double sin = Math.Sin(MathExtensions.ToRad(a_angle));
            double t = 1 - cos;

            double u = a_rotate_around.X;
            double v = a_rotate_around.Y;
            double w = a_rotate_around.Z;

            return new Matrix4(

                t * u * u + cos,
                t * u * v - sin * w,
                t * u * w + sin * v,
                0,

                t * u * v + sin * w,
                t * v * v + cos,
                t * v * w - sin * u,
                0,

                t * u * w - sin * v,
                t * v * w + sin * u,
                t * w * w + cos,
                0,

                0, 0, 0, 1);
        }
            
        public static Matrix4 CreateRotateAroundVector(Vector3 a_rotate_around, double a_cos, double a_sin)
        {
            a_rotate_around = a_rotate_around.Normalized;

            double t = 1 - a_cos;

            double u = a_rotate_around.X;
            double v = a_rotate_around.Y;
            double w = a_rotate_around.Z;

            return new Matrix4(

                t * u * u + a_cos,
                t * u * v - a_sin * w,
                t * u * w + a_sin * v,
                0,

                t * u * v + a_sin * w,
                t * v * v + a_cos,
                t * v * w - a_sin * u,
                0,

                t * u * w - a_sin * v,
                t * v * w + a_sin * u,
                t * w * w + a_cos,
                0,

                0, 0, 0, 1);
        }

        public static Matrix4 CreateScale(Vector3 a_scale)
        {
            return CreateScale(a_scale.X, a_scale.Y, a_scale.Z);
        }

        public static Matrix4 CreateScale(double a_x, double a_y, double a_z)
        {
            return new Matrix4(a_x, 0, 0, 0,
                               0, a_y, 0, 0,
                               0, 0, a_z, 0,
                               0, 0, 0, 1);
        }

        public double[] ToArray()
        {
            return new double[]
            {
                M11, M12, M13, M14,
                M21, M22, M23, M24,
                M31, M32, M33, M34,
                M41, M42, M43, M44
            };
        }
    }
}